//
// Created by HP on 2024/1/13.
//

#include "AuroraCOMM.h"
#include "FOC_thread.h"
#include "rtthread.h"
#include "stdlib.h"

#include "usbd_cdc_if.h"
AuroraCOMM::AuroraCOMM(){
//    state.cmdOrdata = COMM_CMD;
//    state.mstate = IDLE;  //默认为空闲态


    state = IDLE;
}


void AuroraCOMM::recvMsg(uint8_t *data,uint16_t len){
    switch (state) {
        case IDLE:          IDLE_Deal(data,len);break;
        case UPDATE_FILE:   UPDATE_FILE_Deal(data,len);break;
        case M_Calibration: break;
        default: state = IDLE;break;
    }
}

void AuroraCOMM::IDLE_Deal(uint8_t *data,uint16_t len){
    //命令错误
    if(data[0] != CMD_HEAD && data[CMD_LEN-1] != CMD_TAIL) {
        usb_printf("CMD ERROR \r\n"); return;
    }
    else if( len != 24) {
        usb_printf("CMD LEN ERROR \r\n");
        return;
    }
    else
        usb_printf("CMD OK \r\n");
   // struct COMM_MSG_PACKAGE *package = (struct COMM_MSG_PACKAGE *)data;


    if(data[1] == CMD_FILE) //文件更新类
    {
        comm2foc_msg.type = HALT_BOTH_MOTOR;  //暂停两个电机
        comm2foc_msg.cmd = IS_CMD;
        comm2ui_msg.type = file_update;
        comm2ui_msg.file_update.file_type = data[2];
        comm2ui_msg.file_update.file_size = (data[4]<<24) + (data[5]<<16) + (data[6] << 8) + data[7];
        comm2ui_msg.file_update.file_curr_size = 0;
        comm2ui_msg.file_update.file_data_len = 0;
        comm2ui_msg.cmd = IS_CMD;
        state = UPDATE_FILE;
    }
    else if( data[1] == CMD_MOTOR) // 电机控制类
    {
        if( data[2] == CMD_LCalibration)
            comm2foc_msg.type = commenable_LMCalibration;
        else if( data[2] == CMD_RCalibration)
            comm2foc_msg.type = commenable_RMCalibration;
        else if( data[2] == CMD_LRCalibration)
            comm2foc_msg.type = commenable_LRMCalibration;
        else if(data[2] == CMD_OPEN_MODE){
            comm2foc_msg.mode = OPEN_MODE;
            comm2ui_msg.type = mstate_update;
            if( data[3] == CMD_OPEN_LOPEN ) {
                comm2foc_msg.type = LEFT_MOTOR;
                comm2ui_msg.mstate_update.select_motor = 0;
            }
            else if( data[3] == CMD_OPEN_ROPEN) {
                comm2foc_msg.type = RIGHT_MOTOR;
                comm2ui_msg.mstate_update.select_motor = 1;
            }
            else {
                comm2foc_msg.type = BOTH_MOTOR;
                comm2ui_msg.mstate_update.select_motor = 2;
            }
            comm2ui_msg.mstate_update.mode_sel = 4;
            comm2ui_msg.cmd  = IS_CMD;
        }
        else if(data[2] == CMD_I_MODE){
            comm2foc_msg.mode = I_MODE;
            if( data[3] == CMD_I_LI ) {
                comm2foc_msg.type = LEFT_MOTOR;
                comm2ui_msg.mstate_update.select_motor = 0;
            }
            else if( data[3] == CMD_I_RI) {
                comm2foc_msg.type = RIGHT_MOTOR;
                comm2ui_msg.mstate_update.select_motor = 1;
            }
            else {
                comm2foc_msg.type = BOTH_MOTOR;
                comm2ui_msg.mstate_update.select_motor = 2;
            }
            comm2ui_msg.mstate_update.mode_sel = 5;
            comm2ui_msg.cmd  = IS_CMD;
        }
        else if(data[2] == CMD_SPEED_MODE){
            comm2foc_msg.mode = SPEED_MODE;
            if( data[3] == CMD_SPEED_LSPEED ) {
                comm2foc_msg.type = LEFT_MOTOR;
                comm2ui_msg.mstate_update.select_motor = 0;
            }
            else if( data[3] == CMD_SPEED_RSPEED) {
                comm2foc_msg.type = RIGHT_MOTOR;
                comm2ui_msg.mstate_update.select_motor = 1;
            }
            else {
                comm2foc_msg.type = BOTH_MOTOR;
                comm2ui_msg.mstate_update.select_motor = 2;
            }
            comm2ui_msg.mstate_update.mode_sel = 6;
            comm2ui_msg.cmd  = IS_CMD;
        }
        else if(data[2] == CMD_POSITION_MODE){
            comm2foc_msg.mode = POSITION_MODE;
            if( data[3] == CMD_POSITION_LSPEED ) {
                comm2foc_msg.type = LEFT_MOTOR;
                comm2ui_msg.mstate_update.select_motor = 0;
            }
            else if( data[3] == CMD_POSITION_RSPEED) {
                comm2foc_msg.type = RIGHT_MOTOR;
                comm2ui_msg.mstate_update.select_motor = 1;
            }
            else {
                comm2foc_msg.type = BOTH_MOTOR;
                comm2ui_msg.mstate_update.select_motor = 2;
            }
            comm2ui_msg.mstate_update.mode_sel = 7;
            comm2ui_msg.cmd  = IS_CMD;
        }

        else if( data[2] == CMD_I_PARM){  //设置电流环的 目标值 PI值
            float x = 2;
            float *mx = &x;
            unsigned char xbuf[4];
            xbuf[0]=data[4];
            xbuf[1]=data[5];
            xbuf[2]=data[6];
            xbuf[3]=data[7];
            *((unsigned char*)mx + 0) = *(xbuf + 0);
            *((unsigned char*)mx + 1) = *(xbuf + 1);
            *((unsigned char*)mx + 2) = *(xbuf + 2);
            *((unsigned char*)mx + 3) = *(xbuf + 3);
            comm2foc_msg.pid_parm.AIM = *mx;
            xbuf[0]=data[8];
            xbuf[1]=data[9];
            xbuf[2]=data[10];
            xbuf[3]=data[11];
            *((unsigned char*)mx + 0) = *(xbuf + 0);
            *((unsigned char*)mx + 1) = *(xbuf + 1);
            *((unsigned char*)mx + 2) = *(xbuf + 2);
            *((unsigned char*)mx + 3) = *(xbuf + 3);
            comm2foc_msg.pid_parm.P = *mx;
            xbuf[0]=data[12];
            xbuf[1]=data[13];
            xbuf[2]=data[14];
            xbuf[3]=data[15];
            *((unsigned char*)mx + 0) = *(xbuf + 0);
            *((unsigned char*)mx + 1) = *(xbuf + 1);
            *((unsigned char*)mx + 2) = *(xbuf + 2);
            *((unsigned char*)mx + 3) = *(xbuf + 3);
            comm2foc_msg.pid_parm.I = *mx;
            if(data[3] == CMD_IL_AIMI)
                comm2foc_msg.mode = LI_AIM;
            else if(data[3] == CMD_ILQ_PI)
                comm2foc_msg.mode = LI_Qpi;
            else if(data[3] == CMD_ILD_PI)
                comm2foc_msg.mode = LI_Dpi;
            else if(data[3] == CMD_IR_AIMI)
                comm2foc_msg.mode = RI_AIM;
            else if(data[3] == CMD_IRQ_PI)
                comm2foc_msg.mode = RI_Qpi;
            else if(data[3] == CMD_IRD_PI)
                comm2foc_msg.mode = RI_Dpi;
            comm2foc_msg.type = SET_I_PARAM;
        }
        else if( data[2] == CMD_SPEED_PARM){ //设置速度环 参数
            float x = 2;
            float *mx = &x;
            unsigned char xbuf[4];
            xbuf[0]=data[4];
            xbuf[1]=data[5];
            xbuf[2]=data[6];
            xbuf[3]=data[7];
            *((unsigned char*)mx + 0) = *(xbuf + 0);
            *((unsigned char*)mx + 1) = *(xbuf + 1);
            *((unsigned char*)mx + 2) = *(xbuf + 2);
            *((unsigned char*)mx + 3) = *(xbuf + 3);
            comm2foc_msg.pid_parm.AIM = *mx;
            xbuf[0]=data[8];
            xbuf[1]=data[9];
            xbuf[2]=data[10];
            xbuf[3]=data[11];
            *((unsigned char*)mx + 0) = *(xbuf + 0);
            *((unsigned char*)mx + 1) = *(xbuf + 1);
            *((unsigned char*)mx + 2) = *(xbuf + 2);
            *((unsigned char*)mx + 3) = *(xbuf + 3);
            comm2foc_msg.pid_parm.P = *mx;
            xbuf[0]=data[12];
            xbuf[1]=data[13];
            xbuf[2]=data[14];
            xbuf[3]=data[15];
            *((unsigned char*)mx + 0) = *(xbuf + 0);
            *((unsigned char*)mx + 1) = *(xbuf + 1);
            *((unsigned char*)mx + 2) = *(xbuf + 2);
            *((unsigned char*)mx + 3) = *(xbuf + 3);
            comm2foc_msg.pid_parm.I = *mx;
            if(data[3] == CMD_SPEEDL_AIM)
                comm2foc_msg.mode = LSPEED_AIM;
            else if(data[3] == CMD_SPEEDL_PI)
                comm2foc_msg.mode = LSPEED_PI;
            else if(data[3] == CMD_SPEEDR_AIM)
                comm2foc_msg.mode = RSPEED_AIM;
            else if(data[3] == CMD_SPEEDR_PI)
                comm2foc_msg.mode = RSPEED_PI;
            comm2foc_msg.type = SET_SPEED_PARAM;
        }
        else if( data[2] == CMD_POSITION_PARM){ //设置位置环 参数
            float x = 2;
            float *mx = &x;
            unsigned char xbuf[4];
            xbuf[0]=data[4];
            xbuf[1]=data[5];
            xbuf[2]=data[6];
            xbuf[3]=data[7];
            *((unsigned char*)mx + 0) = *(xbuf + 0);
            *((unsigned char*)mx + 1) = *(xbuf + 1);
            *((unsigned char*)mx + 2) = *(xbuf + 2);
            *((unsigned char*)mx + 3) = *(xbuf + 3);
            comm2foc_msg.pid_parm.AIM = *mx;
            xbuf[0]=data[8];
            xbuf[1]=data[9];
            xbuf[2]=data[10];
            xbuf[3]=data[11];
            *((unsigned char*)mx + 0) = *(xbuf + 0);
            *((unsigned char*)mx + 1) = *(xbuf + 1);
            *((unsigned char*)mx + 2) = *(xbuf + 2);
            *((unsigned char*)mx + 3) = *(xbuf + 3);
            comm2foc_msg.pid_parm.P = *mx;
            xbuf[0]=data[12];
            xbuf[1]=data[13];
            xbuf[2]=data[14];
            xbuf[3]=data[15];
            *((unsigned char*)mx + 0) = *(xbuf + 0);
            *((unsigned char*)mx + 1) = *(xbuf + 1);
            *((unsigned char*)mx + 2) = *(xbuf + 2);
            *((unsigned char*)mx + 3) = *(xbuf + 3);
            comm2foc_msg.pid_parm.I = *mx;
            if(data[3] == CMD_POSITIONL_AIM)
                comm2foc_msg.mode = LPOSITION_AIM;
            else if(data[3] == CMD_POSITIONL_PI)
                comm2foc_msg.mode = LPOSITION_PI;
            else if(data[3] == CMD_POSITIONR_AIM)
                comm2foc_msg.mode = RPOSITION_AIM;
            else if(data[3] == CMD_POSITIONR_PI)
                comm2foc_msg.mode = RPOSITION_PI;
            comm2foc_msg.type = SET_POSITION_PARAM;
        }



        else if( data[2] == CMD_STOP_LR){  //暂停电机
            comm2foc_msg.type = HALT_BOTH_MOTOR;
            comm2ui_msg.mstate_update.mode_sel = 3;
            comm2ui_msg.mstate_update.select_motor = 2;
            comm2ui_msg.cmd  = IS_CMD;
        }
        else if( data[2] == CMD_SET_UqUd){  //设置开环的UqUd值
            float x = 2;
            float *mx = &x;
            unsigned char xbuf[4];
            xbuf[0]=data[4];
            xbuf[1]=data[5];
            xbuf[2]=data[6];
            xbuf[3]=data[7];
            *((unsigned char*)mx + 0) = *(xbuf + 0);
            *((unsigned char*)mx + 1) = *(xbuf + 1);
            *((unsigned char*)mx + 2) = *(xbuf + 2);
            *((unsigned char*)mx + 3) = *(xbuf + 3);
            comm2foc_msg.Uq = *mx;
            xbuf[0]=data[8];
            xbuf[1]=data[9];
            xbuf[2]=data[10];
            xbuf[3]=data[11];
            *((unsigned char*)mx + 0) = *(xbuf + 0);
            *((unsigned char*)mx + 1) = *(xbuf + 1);
            *((unsigned char*)mx + 2) = *(xbuf + 2);
            *((unsigned char*)mx + 3) = *(xbuf + 3);

            if(data[3] == CMD_LUqUd)
                comm2foc_msg.mode = LUqUd;
            else
                comm2foc_msg.mode = RUqUd;


            comm2foc_msg.Ud = *mx;
            comm2foc_msg.type = SET_UqUd;
        }
        else if(data[2] == CMD_SET_WaveMaxMin){
            float x = 2;
            float *mx = &x;
            unsigned char xbuf[4];
            xbuf[0]=data[4];
            xbuf[1]=data[5];
            xbuf[2]=data[6];
            xbuf[3]=data[7];
            *((unsigned char*)mx + 0) = *(xbuf + 0);
            *((unsigned char*)mx + 1) = *(xbuf + 1);
            *((unsigned char*)mx + 2) = *(xbuf + 2);
            *((unsigned char*)mx + 3) = *(xbuf + 3);
            comm2foc_msg.wave_max = *mx;
            xbuf[0]=data[8];
            xbuf[1]=data[9];
            xbuf[2]=data[10];
            xbuf[3]=data[11];
            *((unsigned char*)mx + 0) = *(xbuf + 0);
            *((unsigned char*)mx + 1) = *(xbuf + 1);
            *((unsigned char*)mx + 2) = *(xbuf + 2);
            *((unsigned char*)mx + 3) = *(xbuf + 3);
            comm2foc_msg.wave_min = *mx;
            comm2foc_msg.type = SET_WAVEMAXMIN;
            if(data[3] == 0x01)
                comm2foc_msg.mode = LTaTbTc_MAXMIN;
            else if(data[3] == 0x02)
                comm2foc_msg.mode = LIaIbIc_MAXMIN;
            else if(data[3] == 0x03)
                comm2foc_msg.mode = LIqId_MAXMIN;
            else if(data[3] == 0x04)
                comm2foc_msg.mode = LIalphaIbeta_MAXMIN;
            else if(data[3] == 0x05)
                comm2foc_msg.mode = RTaTbTc_MAXMIN;
            else if(data[3] == 0x06)
                comm2foc_msg.mode = RIaIbIc_MAXMIN;
            else if(data[3] == 0x07)
                comm2foc_msg.mode = RIqId_MAXMIN;
            else
                comm2foc_msg.mode = RIalphaIbeta_MAXMIN;
        }
        comm2foc_msg.cmd = IS_CMD;
    }
}

void AuroraCOMM::UPDATE_FILE_Deal(uint8_t *data,uint16_t len){
    comm2ui_msg.file_update.file_curr_size += len;
    comm2ui_msg.file_update.file_data_len = len;
    memcpy(comm2ui_msg.file_update.file_data,data,len);
    comm2ui_msg.cmd = IS_DATA;
    if( comm2ui_msg.file_update.file_type == CMD_ANIM_IMG ||  comm2ui_msg.file_update.file_type == CMD_IMG) {
        if( comm2ui_msg.file_update.file_curr_size >= (comm2ui_msg.file_update.file_size * (172*320*2))){
            usb_printf("file update ok!");
            state = IDLE;
        }
        else
            ;
    }
    else if( comm2ui_msg.file_update.file_curr_size >= comm2ui_msg.file_update.file_size){
        usb_printf("file update ok!");
        state = IDLE;
    }
}




void AuroraCOMM::dealmsgEvent_from_FOC(struct Aurora_FOC2COMM_MSG *foc2comm_msg_p){

    if( foc2comm_msg_p->type == foc2comm_wave_data){
        sendMsg(0xA1,0xFF,0xFF,(union float_uint32_int_char *)&foc2comm_msg_p->waveData.data1,
                (union float_uint32_int_char *)&foc2comm_msg_p->waveData.data2,
                (union float_uint32_int_char *)&foc2comm_msg_p->waveData.data3,
                (union float_uint32_int_char *)&foc2comm_msg_p->waveData.data4);
    }

}
void AuroraCOMM::sendMsg(uint8_t func1,uint8_t func2,uint8_t func3,union float_uint32_int_char *data1,union float_uint32_int_char *data2,union float_uint32_int_char *data3,union float_uint32_int_char *data4){
    struct COMM_MSG_PACKAGE package;
    package.head = CMD_HEAD;
    package.tail = CMD_TAIL;

    package.func1 = func1;
    package.func2 = func2;
    package.func3 = func3;

    package.data1 = *data1;
    package.data2 = *data2;
    package.data3 = *data3;
    package.data4 = *data4;

    CDC_Transmit_FS((uint8_t *)&package, sizeof (struct COMM_MSG_PACKAGE));
}
